﻿// Auhtor: Hristo Hristov
// Date: 30.05.11
// Revision 11

// Version 1.0: (01.06.11) Hand Tracker
// Version 1.1: (07.06.11) Skeleton Tracker
// Version 1.2: (01.08.11) Object Scan

// Version 2: (01.10.11) Changed from OpenNI to Microsoft SDK



#define SDK 
//#define KINECT_OPENNI

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
#if KINECT_OPENNI
using OpenNI;
using NITE;
#endif
using Microsoft.Xna.Framework.Graphics;
using System.Threading;
using Microsoft.Xna.Framework;
using NOVA.Utilities;
using Microsoft.Research.Kinect.Nui;
using NOVA.UI;
using Microsoft.Research.Kinect.Audio;
using Microsoft.Speech.Recognition;



namespace NOVA.Components.Kinect
{
    #region Enums

    /// <summary>
    /// Enum defining the different state, in which 
    /// the tracking can be
    /// </summary>
    public enum KinectState
    {
        IN_SESSION,
	    NOT_IN_SESSION,
	    QUICK_REFOCUS	
    }

    /// <summary>
    /// Enum defining the different Kinect application modes
    /// </summary>
    public enum KinectApplication
    {
        HandTracker,
        StickFigure,
        ObjectScan
    }

    #endregion
#if KINECT_OPENNI
    #region Structs
        
    /// <summary>
    /// Struct defining all skeleton joint transformation
    /// </summary>
    public struct KinectSkeleton
    {
        public SkeletonJointTransformation Head;
        //public SkeletonJointTransformation LeftAnkle;
        //public SkeletonJointTransformation LeftCollar;
        public SkeletonJointTransformation LeftEllbow;
        //public SkeletonJointTransformation LeftFingertip;
        public SkeletonJointTransformation LeftFoot;
        public SkeletonJointTransformation LeftHand;
        public SkeletonJointTransformation LeftHip;
        public SkeletonJointTransformation LeftKnee;
        public SkeletonJointTransformation LeftShoulder;
        //public SkeletonJointTransformation LeftWrist;
        public SkeletonJointTransformation Neck;
        //public SkeletonJointTransformation RightAnkle;
        //public SkeletonJointTransformation RightCollar;
        public SkeletonJointTransformation RightEllbow;
        //public SkeletonJointTransformation RightFingertip;
        public SkeletonJointTransformation RightFoot;
        public SkeletonJointTransformation RightHand;
        public SkeletonJointTransformation RightHip;
        public SkeletonJointTransformation RightKnee;
        public SkeletonJointTransformation RightShoulder;
        //public SkeletonJointTransformation RightWrist;
        public SkeletonJointTransformation Torso;
        //public SkeletonJointTransformation Waist;
    }

    #endregion
    
    public class Kinect : IDisposable
    {        
    #region Members

        KinectState m_state;
        KinectApplication m_appState;

        Dictionary<int, KinectSkeleton> m_userSkeletons;

        KinectSkeleton m_initialSkeleton;

        SkeletonCapability m_SkeletonCapability;
        PoseDetectionCapability m_PoseDetectionCapability;

        Point3D m_initialPoint;

        int m_iFPS;

        bool m_bTrackSkeleton;
        bool m_bInitiating;

        Vector3 m_handPosition;

        Texture2D m_textureDepth;
        Texture2D m_textureRGB;     // TODO

        bool threadRunning;
        ManualResetEvent exitEvent;

        Thread m_thread;

        IntPtr m_imgHandle;

        Emgu.CV.Image<Emgu.CV.Structure.Gray, ushort> img = new Emgu.CV.Image<Emgu.CV.Structure.Gray, ushort>(640, 480);

        // OpenNI objects
        OpenNI.Context m_context;
        OpenNI.DepthGenerator m_depthGenerator;
        OpenNI.HandsGenerator m_handsGenerator;
        OpenNI.UserGenerator m_userGenerator;

        // NITE objects
        NITE.SessionManager m_sessionManager;
        NITE.FlowRouter m_flowRouter;        

        #endregion

    #region Properties
        public Texture2D TextureDepth { get { return m_textureDepth; } }
        public Texture2D TextureRGB { get { return m_textureRGB; } }
        public KinectState SessionState { get { return m_state; } }
        public Vector3 HandPosition { get { return m_handPosition; } }
        public Dictionary<int, KinectSkeleton> Skeletons { get { return m_userSkeletons; } }
        public bool IsTrackingPose { get { return m_bTrackSkeleton; } }
        public int FPS { get { return m_iFPS; } }
        public IntPtr ImgHandle { get { return m_imgHandle; } }
        #endregion

    #region Constructor
        public Kinect(KinectApplication appType)
        {
            m_appState = appType;            

            // Initialize OpenNI
            switch (appType)
            {
                case KinectApplication.HandTracker:
                    // Create the context
                    m_context = new Context("C:\\Sample-Tracking.xml" /* TODO */);

                    // Create the depth generator for the depth image
                    m_depthGenerator = (OpenNI.DepthGenerator)m_context.FindExistingNode(NodeType.Depth);
                    

                    // Create the hand generator, which is used for the hand tracking
                    m_handsGenerator = (OpenNI.HandsGenerator) m_context.FindExistingNode(NodeType.Hands);

                    // Create the session manager
                    m_sessionManager = new SessionManager(m_context, "Click,Wave", "RaiseHand");

                    // Set the session manager callbacks
                    m_sessionManager.SessionStart += delegate { m_state = KinectState.IN_SESSION; };//new EventHandler<PositionEventArgs>(SessionStarting);
                    m_sessionManager.SessionEnd += delegate { m_state = KinectState.NOT_IN_SESSION; };//new EventHandler(SessionEnd);
                    m_sessionManager.SessionFocusProgress += delegate { };//new EventHandler<SessionProgressEventArgs>(SessionFocusProgress);

                    // Create the flow router
                    m_flowRouter = new FlowRouter();

                    // Assign the flow router to the session manager
                    m_sessionManager.AddListener(m_flowRouter);

                    // Set the hand tracker callback
                    m_handsGenerator.HandUpdate += new EventHandler<HandUpdateEventArgs>(m_handsGenerator_HandUpdate);

                    // Initialization done. Start generating
                    m_context.StartGeneratingAll();

                    break;
                case KinectApplication.StickFigure: 
                    // Create the context
                    m_context = new Context("C:\\Sample-User.xml" /* TODO */);

                    // Create the depth generator for the depth image
                    m_depthGenerator = (OpenNI.DepthGenerator)m_context.FindExistingNode(NodeType.Depth);
                    
                    // Create the user generator
                    m_userGenerator = (OpenNI.UserGenerator)m_context.FindExistingNode(NodeType.User);

                    if (!m_userGenerator.IsCapabilitySupported(Capabilities.Skeleton) ||
                        !m_userGenerator.IsCapabilitySupported(Capabilities.PoseDetection))
                    {
                        throw new Exception("User generator doesn't support either skeleton or pose detection.");
                        return;
                    }
                    else
                    {
                        // THIS IS VERY IMPORTANT:
                        // To avoid the "CallbackOnCollectedDelegate"-exception,
                        // when processing the unmanaged callbacks of the skeleton
                        // capability and the pose detection.
                        m_SkeletonCapability = m_userGenerator.SkeletonCapability;
                        m_PoseDetectionCapability = m_userGenerator.PoseDetectionCapability;
                    }

                    // Set the desired skeleton profile to be tracked
                    m_SkeletonCapability.SetSkeletonProfile(SkeletonProfile.All);

                    // Initialization done. Start generating
                    m_context.StartGeneratingAll();
                    
                    // Set the user generator callbacks
                    m_userGenerator.NewUser += new EventHandler<NewUserEventArgs>(m_userGenerator_NewUser);                    
                    m_userGenerator.LostUser += new EventHandler<UserLostEventArgs>(m_userGenerator_LostUser);
                    m_SkeletonCapability.CalibrationStart += new EventHandler<CalibrationStartEventArgs>(SkeletonCapability_CalibrationStart);
                    m_SkeletonCapability.CalibrationEnd += new EventHandler<CalibrationEndEventArgs>(SkeletonCapability_CalibrationEnd);
                    m_PoseDetectionCapability.PoseDetected += new EventHandler<PoseDetectedEventArgs>(PoseDetectionCapability_PoseDetected);
                    
                    // Create the skeleton dictionary, which holds all skeleton joint transformations of all users
                    m_userSkeletons = new Dictionary<int, KinectSkeleton>();

                    m_bTrackSkeleton = false;

                    m_bInitiating = true;

                    break;
                case KinectApplication.ObjectScan:
                    // Create the context
                    m_context = new Context("C:\\Sample-User.xml" /* TODO */);

                    // Create the depth generator for the depth image
                    m_depthGenerator = (OpenNI.DepthGenerator)m_context.FindExistingNode(NodeType.Depth);

                    // Activate the (hardware) registration to connect the depth and the rgb data
                    m_depthGenerator.SetIntProperty("RegistrationType", 2); 
                    break;
                default:
                    break;
            }        

            m_state = KinectState.NOT_IN_SESSION;

            //// Start the image processing threading
            //exitEvent = new ManualResetEvent(false);
            //ThreadPool.QueueUserWorkItem(Update);

            //threadRunning = true;
            //exitEvent.WaitOne(3000);

            m_thread = new Thread(Update);
            threadRunning = true;
            m_thread.Start();
        }
        #endregion

    #region Public Methods

        /// <summary>
        /// Thread routine, which does all the routine
        /// </summary>
        public void Update()//object obj)
        {            
            //threadRunning = true;

            while (threadRunning)
            {
                OpenNI.MapOutputMode mode;

                // Read next available data
                m_context.WaitOneUpdateAll(m_depthGenerator);
                
                // Create the background texture containing the depth image
                m_textureDepth = CreateDepthTexture();

                switch (m_appState)
                {
                    case KinectApplication.HandTracker:
                        // Update NITE tree
                        m_sessionManager.Update(m_context);                        
                        break;
                    case KinectApplication.StickFigure:
                        // Assign the players
                        /*if (m_userSkeletons.Count > 0)
                        {
                            int id = 0;
                            m_userGenerator.GetCoM(id);

                            if (id == 0)
                                FindPlayer();
                        }*/

                        if(m_bTrackSkeleton)
                            UpdateStickFigure();
                        
                        break;
                }

                
            }

            //exitEvent.Set();
        }
        
        /// <summary>
        /// Create a texture from the depth image for the scene background
        /// </summary>
        public Texture2D CreateDepthTexture()
        {
            OpenNI.DepthMetaData metaData = new DepthMetaData();
            m_depthGenerator.GetMetaData(metaData);

            m_imgHandle = metaData.DepthMapPtr;
            
            int iWidth = metaData.XRes;
            int iHeight = metaData.YRes;

            int iNewWidth = GetClosestPowerOfTwo(iWidth);
            int iNewHeight = GetClosestPowerOfTwo(iHeight);

            int MAX_DEPTH = 10000;

            

            short nValue = 0;
            uint nHistValue = 0;
            int nIndex = 0;
            int nX = 0;
            int nY = 0;
            int nNumberOfPoints = 0;

            int[] array = new int[iWidth * iHeight];
            float[] pDepthHist = new float[MAX_DEPTH];

            // Calculate the accumulative histogram
            unsafe
            {
                short* pDepth1 = (short*)(metaData.DepthMapPtr);
                for (nY = 0; nY < iHeight; nY++)
                {
                    for (nX = 0; nX < iWidth; nX++)
                    {
                        nValue = *pDepth1;

                        if (nValue != 0)
                        {
                            pDepthHist[nValue]++;
                            nNumberOfPoints++;
                        }

                        pDepth1++;
                    }
                }
            }

            for (nIndex = 1; nIndex < MAX_DEPTH; nIndex++)
            {
                pDepthHist[nIndex] += pDepthHist[nIndex - 1];
            }
            
            if (nNumberOfPoints > 0)
            {
                for (nIndex = 1; nIndex < MAX_DEPTH; nIndex++)
                {
                    pDepthHist[nIndex] = (uint)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
                }
            }
            
            // Create the texture array
            unsafe
            {
                short* pDepth = (short*)(metaData.DepthMapPtr);
                
                // Determine the image stride
                int srcStride = iWidth * 4;
                int index = 0;
                for (int i = 0; i < iHeight; i++)
                {
                    for (int j = 0; j < iWidth; j++)
                    {
                        nValue = *pDepth;
                        if (nValue != 0)
                        {
                            nHistValue = (uint)pDepthHist[nValue];
                            array[index++] = (int)((nHistValue << 16) | ((nHistValue + 1) << 8) | nHistValue);                        
                        }
                        else
                            array[index++] = (int)0;

                        pDepth++;
                    }
                    nIndex += (iNewWidth - iWidth);
                }
            }

            
            // Create the actual texture from the data array
            Texture2D texture = new Texture2D(Core.Device, iWidth, iHeight, false, SurfaceFormat.Color);
            texture.SetData<int>(array, 0, iWidth * iHeight);
            
            return texture;
        }

        public void Dispose()
        {
            m_thread.Abort();
            threadRunning = false;
            
            //m_sessionManager.EndSession();
            //m_context.Shutdown();            
        }
        #endregion

    #region Private Methods

        int GetClosestPowerOfTwo(int n)
        {
	        int m = 2;
	        while(m < n) 
                m<<=1;

	        return m;
        }

        void UpdateStickFigure()
        {
            int nXRes;
	        int nYRes;

	        DepthMetaData dm = new DepthMetaData();
	        m_depthGenerator.GetMetaData(dm);

            m_iFPS = dm.FPS;

	        Point3D corner = new Point3D(dm.XRes, dm.YRes, dm.ZRes);
	        m_depthGenerator.ConvertProjectiveToRealWorld(corner);

	        nXRes = dm.XRes;
	        nYRes = dm.YRes;

            int[] users;

	        users = m_userGenerator.GetUsers();
	        for (int i = 0; i < users.Count<int>(); ++i)
	        {
		        if (m_userGenerator.SkeletonCapability.IsTracking(users[i]))
		        {
			        UpdateSingleUser(users[i]);
		        }
	        }
        }

        void UpdateSingleUser(int user)
        {
            KinectSkeleton skeleton = new KinectSkeleton();
            
#if OLD
            // Get all joint transformations of the skeleton
            //skeleton.LeftAnkle      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftAnkle);
            //skeleton.LeftCollar     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftCollar);
            skeleton.LeftEllbow = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftElbow);
            //skeleton.LeftFingertip  = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFingertip);
            skeleton.LeftFoot = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFoot);
            skeleton.LeftHand = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHand);
            skeleton.LeftHip = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHip);
            skeleton.LeftKnee = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftKnee);
            skeleton.LeftShoulder = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftShoulder);
            //skeleton.LeftWrist      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftWrist);
            skeleton.Neck = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Neck);
            //skeleton.RightAnkle     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightAnkle);
            //skeleton.RightCollar    = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightCollar);
            skeleton.RightEllbow = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightElbow);
            //skeleton.RightFingertip = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFingertip);
            skeleton.RightFoot = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFoot);
            skeleton.RightHand = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHand);
            skeleton.RightHip = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHip);
            skeleton.RightKnee = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightKnee);
            skeleton.RightShoulder = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightShoulder);
            //skeleton.RightWrist     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightWrist);
            skeleton.Torso = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Torso);
            //skeleton.Waist          = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Waist);      
#endif

#if OLD2
            skeleton.Head = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Head), m_initialSkeleton.Head);
            //skeleton.LeftAnkle      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftAnkle);
            //skeleton.LeftCollar     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftCollar);
            skeleton.LeftEllbow = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftElbow), m_initialSkeleton.LeftEllbow);
            //skeleton.LeftFingertip  = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFingertip);
            skeleton.LeftFoot = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFoot), m_initialSkeleton.LeftFoot);
            skeleton.LeftHand = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHand), m_initialSkeleton.LeftHand);
            skeleton.LeftHip = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHip), m_initialSkeleton.LeftHip);
            skeleton.LeftKnee = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftKnee), m_initialSkeleton.LeftKnee);
            skeleton.LeftShoulder = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftShoulder), m_initialSkeleton.LeftShoulder);
            //skeleton.LeftWrist      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftWrist);
            skeleton.Neck = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Neck), m_initialSkeleton.Neck);
            //skeleton.RightAnkle     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightAnkle);
            //skeleton.RightCollar    = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightCollar);
            skeleton.RightEllbow = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightElbow), m_initialSkeleton.RightEllbow);
            //skeleton.RightFingertip = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFingertip);
            skeleton.RightFoot = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFoot), m_initialSkeleton.RightFoot);
            skeleton.RightHand = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHand), m_initialSkeleton.RightHand);
            skeleton.RightHip = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHip), m_initialSkeleton.RightHip);
            skeleton.RightKnee = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightKnee), m_initialSkeleton.RightKnee);
            skeleton.RightShoulder = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightShoulder), m_initialSkeleton.RightShoulder);
            //skeleton.RightWrist     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightWrist);
            skeleton.Torso = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Torso), m_initialSkeleton.Torso);
            //skeleton.Waist          = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Waist);            
            

            // Save this transformations either as initial or regular
            if (m_bInitiating == false)
                m_userSkeletons[user] = skeleton;
            else
                m_initialSkeleton = skeleton;

            if(m_initialSkeleton.Head.Position.Position.X > 0 || m_initialSkeleton.Head.Position.Position.Y > 0 || m_initialSkeleton.Head.Position.Position.Z > 0)
                m_bInitiating = false;

            Console.WriteLine(String.Format("{0} | {1} | {2}", m_initialSkeleton.Neck.Position.Position.X, m_initialSkeleton.Neck.Position.Position.Y, m_initialSkeleton.Neck.Position.Position.Z));
#endif
            skeleton.Head = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Head), m_initialSkeleton.Head);
            //skeleton.LeftAnkle      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftAnkle);
            //skeleton.LeftCollar     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftCollar);
            skeleton.LeftEllbow = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftElbow), m_initialSkeleton.LeftEllbow);
            //skeleton.LeftFingertip  = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFingertip);
            skeleton.LeftFoot = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftFoot), m_initialSkeleton.LeftFoot);
            skeleton.LeftHand = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHand), m_initialSkeleton.LeftHand);
            skeleton.LeftHip = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftHip), m_initialSkeleton.LeftHip);
            skeleton.LeftKnee = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftKnee), m_initialSkeleton.LeftKnee);
            skeleton.LeftShoulder = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftShoulder), m_initialSkeleton.LeftShoulder);
            //skeleton.LeftWrist      = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.LeftWrist);
            skeleton.Neck = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Neck), m_initialSkeleton.Neck);
            //skeleton.RightAnkle     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightAnkle);
            //skeleton.RightCollar    = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightCollar);
            skeleton.RightEllbow = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightElbow), m_initialSkeleton.RightEllbow);
            //skeleton.RightFingertip = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFingertip);
            skeleton.RightFoot = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightFoot), m_initialSkeleton.RightFoot);
            skeleton.RightHand = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHand), m_initialSkeleton.RightHand);
            skeleton.RightHip = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightHip), m_initialSkeleton.RightHip);
            skeleton.RightKnee = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightKnee), m_initialSkeleton.RightKnee);
            skeleton.RightShoulder = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightShoulder), m_initialSkeleton.RightShoulder);
            //skeleton.RightWrist     = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.RightWrist);
            skeleton.Torso = ModifyJoint(m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Torso), m_initialSkeleton.Torso);
            //skeleton.Waist          = m_SkeletonCapability.GetSkeletonJoint(user, SkeletonJoint.Waist);            


            // Save this transformations either as initial or regular
            //if (m_bInitiating == false)
                m_userSkeletons[user] = skeleton;
            //else
            //    m_initialSkeleton = skeleton;

            if (m_initialSkeleton.Head.Position.Position.X > 0 || m_initialSkeleton.Head.Position.Position.Y > 0 || m_initialSkeleton.Head.Position.Position.Z > 0)
                m_bInitiating = false;
        }

        void FindPlayer()
        {
            int[] user = m_userGenerator.GetUsers();

            for (int i = 0; i < user.Count<int>(); i++)
                if (AssignPlayer(user[i]))
                    return;
        }

        bool AssignPlayer(int user)
        {
            Point3D com;
            com = m_userGenerator.GetCoM(user);

            if (com.Z == 0)
                return false;

            m_userGenerator.SkeletonCapability.LoadCalibrationData(user, 0);
            m_userGenerator.SkeletonCapability.StartTracking(user);
            
            return true;
        }

        SkeletonJointTransformation ModifyJoint(SkeletonJointTransformation current, SkeletonJointTransformation initial)
        {
            SkeletonJointTransformation transformation = new SkeletonJointTransformation();
            SkeletonJointPosition j = new SkeletonJointPosition();
            SkeletonJointOrientation o = new SkeletonJointOrientation();
            Point3D p;
            /*
            if(m_bInitiating == false)
                p = new Point3D((float)(current.Position.Position.X / initial.Position.Position.X),
                                (float)(current.Position.Position.Y / initial.Position.Position.Y),
                                (float)(current.Position.Position.Z / initial.Position.Position.Z));
            else*/
                p = new Point3D(current.Position.Position.X,
                                current.Position.Position.Y,
                                current.Position.Position.Z);

            p = m_depthGenerator.ConvertRealWorldToProjective(p);                       

            //current.Orientation = initial.Orientation * current.Orientation; 

            OpenNI.DepthMetaData metaData = new DepthMetaData();
            m_depthGenerator.GetMetaData(metaData);

            //if (m_bInitiating == false)
            //    j.Position = new Point3D((p.X / (float)metaData.XRes) - m_initialPoint.X, (p.Y / (float)metaData.YRes) - m_initialPoint.Y, (p.Z / (float)metaData.ZRes) - m_initialPoint.Z);
            //else
            {
                m_initialPoint = new Point3D((p.X / (float)metaData.XRes), (p.Y / (float)metaData.YRes), (p.Z / (float)metaData.ZRes));
                j.Position = m_initialPoint;
            }
            
            
            //j.Position = new Point3D(p.X, p.Y, p.Z);
                      

            transformation.Position = j;
            transformation.Orientation = o;

            return transformation;
        }

        #endregion

    #region Events

        void m_handsGenerator_HandUpdate(object o, HandUpdateEventArgs e)
        {
            Vector3 vec = new Vector3();
            Point3D point = e.Position;

            point = m_depthGenerator.ConvertRealWorldToProjective(point);

            OpenNI.DepthMetaData metaData = new DepthMetaData();
            m_depthGenerator.GetMetaData(metaData);

            m_iFPS = metaData.FPS;

            // Return the 2D coordinates independed of the image size
            vec.X = point.X / (float)metaData.XRes;
            vec.Y = point.Y / (float)metaData.YRes;

            // Return the actual depth
            vec.Z = point.Z / (float)metaData.ZRes;

            m_handPosition = vec;
        }

        void m_userGenerator_NewUser(object o, NewUserEventArgs e)
        {
            m_userGenerator.PoseDetectionCapability.StartPoseDetection("Psi", e.ID);

            AssignPlayer(e.ID);

            m_userSkeletons.Add(e.ID, new KinectSkeleton());
        }

        void m_userGenerator_LostUser(object o, UserLostEventArgs e)
        {
            m_userSkeletons.Remove(e.ID);

            // TODO: Message output
        }

        void SkeletonCapability_CalibrationStart(object o, CalibrationStartEventArgs e)
        {
            //FindPlayer();
            // TODO: Message output
        }

        void SkeletonCapability_CalibrationEnd(object o, CalibrationEndEventArgs e)
        {
            if (e.Success)
                m_userGenerator.SkeletonCapability.StartTracking(e.ID);
            else
                m_userGenerator.PoseDetectionCapability.StartPoseDetection("Psi", e.ID);

            // Save the initial joint transformation
            UpdateSingleUser(e.ID);
            m_bTrackSkeleton = true;             
        }

        void PoseDetectionCapability_PoseDetected(object o, PoseDetectedEventArgs e)
        {
            m_userGenerator.SkeletonCapability.RequestCalibration(e.ID, true);
            m_userGenerator.PoseDetectionCapability.StopPoseDetection(e.ID);
        }

        #endregion
    }
#endif

#if SDK
    public class JointInfo
    {
        /// <summary>
        /// Returns the normalized screen position. Range between 0 and 1.
        /// </summary>
        public Vector2 ScreenPosition;
        public Vector3 WorldPosition;
        public JointTrackingState TrackingState;

        public JointInfo(Vector2 screen, Vector3 world, JointTrackingState state)
        {
            ScreenPosition = screen;
            WorldPosition = world;
            TrackingState = state;
        }
    }

    public class Skeleton
    {
        public Dictionary<JointID, JointInfo> Joints
        {
            get;
            internal set;
        }

        public Vector3 WorldPosition
        {
            get;
            internal set;
        }

        public Vector2 ScreenPosition
        {
            get;
            internal set;
        }

        public SkeletonQuality Quality
        {
            get;
            internal set;
        }

        public int TrackingID
        {
            get;
            internal set;
        }

        public SkeletonTrackingState TrackingState
        {
            get;
            set;
        }

        public int UserIndex
        {
            get;
            internal set;
        }
    }

    public class Kinect : IDisposable
    {
        public enum KinectCameraImage
        {
            /// <summary>
            /// No kinect image is shown.
            /// </summary>
            None = 0,
            /// <summary>
            /// The depth image of the kinect is shown.
            /// </summary>
            Depth,
            /// <summary>
            /// The RGB image of the kinect is shown.
            /// </summary>
            RGB,
            /// <summary>
            /// A reduced version of the RGB image of the kinect is shown,
            /// which shows only the active players using alpha blend.
            /// </summary>
            ReducedRGB
        }

        #region Members
        Runtime nui;
        int totalFrames = 0;
        int lastFrames = 0;
        DateTime lastTime = DateTime.MaxValue;
        SkeletonFrame m_skeletonFrame;
        
        byte[] m_hollowFrame = new byte[640 * 480];

        public int m_iFPS;

        Texture2D m_texColor;
        Texture2D m_texDepth;

        // We want to control how depth data gets converted into false-color data
        // for more intuitive visualization, so we keep 32-bit color frame buffer versions of
        // these, to be updated whenever we receive and process a 16-bit frame.
        const int RED_IDX = 2;
        const int GREEN_IDX = 1;
        const int BLUE_IDX = 0;
        byte[] depthFrame32 = new byte[320 * 240 * 4];

        //private List<Dictionary<JointID, JointInfo>> skeletons;
        private List<Skeleton> m_skeletons;

        Thread thread;
        
        #endregion
        
        #region Properties

        /// <summary>
        /// Gets the skeleton engine of the Kinect SDK. 
        /// This can be used to adjust the smoothing parameters.
        /// </summary>
        public SkeletonEngine SkeletonEngine
        {
            get { return nui.SkeletonEngine; }
        }

        /// <summary>
        /// Gets the RGB video image.
        /// </summary>
        public Texture2D TextureColor { get { return m_texColor; } }

        /// <summary>
        /// Gets the depth video image.
        /// </summary>
        public Texture2D TextureDepth { get { return m_texDepth; } }

        // TO_DELETE >
        public Vector3 HandPosition { get; set; }
        //public Dictionary<int, KinectSkeleton> Skeletons { get; set; }
        public bool IsTrackingPose { get; set; }
        // TO_DELETE <              

        public Skeleton Player1 { get { return m_skeletons[0]; } }
        public Skeleton Player2 { get { return m_skeletons[1]; } }
        public Skeleton Player3 { get { return m_skeletons[2]; } }
        public Skeleton Player4 { get { return m_skeletons[3]; } }
        public Skeleton Player5 { get { return m_skeletons[4]; } }
        public Skeleton Player6 { get { return m_skeletons[5]; } }
        public Skeleton Player7 { get { return m_skeletons[6]; } }
        public Skeleton Player8 { get { return m_skeletons[7]; } }

        /// <summary>
        /// Gets a list of all skeletons.
        /// </summary>
        public List<Skeleton> Skeletons { get { return m_skeletons; } }

        /// <summary>
        /// Gets amount of tracked skeletons.
        /// </summary>
        public int SkeletonCount { get { return m_skeletons.Count;  } }

        /// <summary>
        /// Set if skeleton data is ready.
        /// </summary>
        public bool SkeletonDataReady { get { return (m_skeletonFrame != null); } }

        /// <summary>
        /// If enabled, the camera image is shown.
        /// </summary>
        public KinectCameraImage ShowCameraImage { get; set; }

        #endregion

        #region Constructor
        public Kinect(GraphicsDevice graphicsDevice, RuntimeOptions options)
        {
            

            try
            {
                nui = new Runtime();
                nui.Initialize(options);
            }
            catch (InvalidOperationException)
            {
                ExceptionManager.Add(new Exception("Runtime initialization failed. Please make sure Kinect device is plugged in."));
                return;
            }

            m_texDepth = new Texture2D(graphicsDevice, 320, 240, false, SurfaceFormat.Color);
            m_texColor = new Texture2D(graphicsDevice, 640, 480, false, SurfaceFormat.Color);

            //skeletons = new List<Dictionary<JointID, JointInfo>>();
            m_skeletons = new List<Skeleton>();

            // Initiate this list with dummy entries
            for (int i = 0; i < 8; i++)
                m_skeletons.Add(new Skeleton()); 
           

            try
            {
                nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);                
            }
            catch (InvalidOperationException)
            {
                ExceptionManager.Add(new Exception("Failed to open stream. Please make sure to specify a supported image type and resolution."));
                return;
            }

            lastTime = DateTime.Now;

            int iOptions = (int)options;
            int iColor = (int)RuntimeOptions.UseColor;
            int iDepth1 = (int)RuntimeOptions.UseDepth;
            int iDepth2 = (int)RuntimeOptions.UseDepthAndPlayerIndex;
            int iSkeleton = (int)RuntimeOptions.UseSkeletalTracking;

            if ((iOptions & iDepth1) == iDepth1 || (iOptions & iDepth2) == iDepth2)
                nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
            
            if ((iOptions & iSkeleton) == iSkeleton)
                nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);

            if ((iOptions & iColor) == iColor)
                nui.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_ColorFrameReady);

            TransformSmoothParameters param = new TransformSmoothParameters();
            param.Correction = 0.05f;
            param.JitterRadius = 0.9f;
            param.MaxDeviationRadius = 0.55f;
            param.Prediction = 0.005f;
            param.Smoothing = 0.001f;

            nui.SkeletonEngine.SmoothParameters = param;

            m_iFPS = 0;
            ShowCameraImage = KinectCameraImage.None;

            //thread = new Thread(Update);
            //thread.Start();
        }
        #endregion  

        //public void Update()
        //{
        //    ImageFrame img;
        //    ImageFrame depth;
        //    SkeletonFrame ske;

        //    while (true)
        //    {
        //        img = nui.DepthStream.GetNextFrame(100);

        //        PlanarImage Image = img.Image;
        //        byte[] convertedDepthFrame = convertDepthFrame(Image.Bits);

        //        m_texDepth.SetData<byte>(convertedDepthFrame);


        //        ++totalFrames;

        //        DateTime cur = DateTime.Now;
        //        if (cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
        //        {
        //            int frameDiff = totalFrames - lastFrames;
        //            lastFrames = totalFrames;
        //            lastTime = cur;

        //            m_iFPS = frameDiff;
        //        }

        //        // -----------------------------------------------------
        //        depth = nui.VideoStream.GetNextFrame(100);

        //        // 32-bit per pixel, RGBA image
        //        Image = depth.Image;

        //        byte[] newImage = new byte[Image.Width * Image.Height * Image.BytesPerPixel];

        //        // Change from ARGB- to RGBA-format
        //        for (int i = 0; i < newImage.Length; i += 4)
        //        {
        //            newImage[i + 2] = Image.Bits[i + 0];
        //            newImage[i + 1] = Image.Bits[i + 1];
        //            newImage[i + 0] = Image.Bits[i + 2];
        //        }

        //        m_texColor.SetData<byte>(newImage);


        //        // -----------------------------------------------------
        //        ske = nui.SkeletonEngine.GetNextFrame(100);

        //        m_skeletonFrame = ske;

        //        //m_skeletons.Clear();

        //        for (int i = 0; i < ske.Skeletons.Length; i++)            //foreach (SkeletonData data in skeletonFrame.Skeletons)
        //        {
        //            Skeleton skeleton = new Skeleton();

        //            // Set all skeleton data
        //            skeleton.WorldPosition = GetWorldVector3(ske.Skeletons[i].Position);//data.Position);
        //            skeleton.ScreenPosition = GetDisplayPosition(ske.Skeletons[i].Position);//data.Position);
        //            skeleton.Quality = ske.Skeletons[i].Quality;//data.Quality;
        //            skeleton.TrackingID = ske.Skeletons[i].TrackingID;//data.TrackingID;
        //            skeleton.TrackingState = ske.Skeletons[i].TrackingState;//data.TrackingState;
        //            skeleton.UserIndex = ske.Skeletons[i].UserIndex;//data.UserIndex;

        //            Dictionary<JointID, JointInfo> joints = new Dictionary<JointID, JointInfo>();

        //            // Process all joints
        //            if (SkeletonTrackingState.Tracked == ske.Skeletons[i].TrackingState)//data.TrackingState)
        //            {
        //                foreach (Joint joint in ske.Skeletons[i].Joints)//data.Joints)
        //                {
        //                    joints.Add(joint.ID, new JointInfo(GetDisplayPosition(joint.Position), GetWorldVector3(joint.Position), joint.TrackingState));
        //                }
        //            }

        //            skeleton.Joints = joints;

        //            // Add this skeleton to the known ones
        //            m_skeletons[i] = skeleton;//Add(skeleton);
        //        } 
        //    }
        //}

        #region Events
        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage Image = e.ImageFrame.Image;
            byte[] convertedDepthFrame = convertDepthFrame(Image.Bits);
            
            m_texDepth.SetData<byte>(convertedDepthFrame);
            
            
            ++totalFrames;

            DateTime cur = DateTime.Now;
            if (cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
            {
                int frameDiff = totalFrames - lastFrames;
                lastFrames = totalFrames;
                lastTime = cur;

                m_iFPS = frameDiff;
            }
        }

        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            SkeletonFrame skeletonFrame = e.SkeletonFrame;
            m_skeletonFrame = skeletonFrame;

            //m_skeletons.Clear();
            
            for(int i=0;i<skeletonFrame.Skeletons.Length; i++)            //foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                Skeleton skeleton = new Skeleton();
                
                // Set all skeleton data
                skeleton.WorldPosition = GetWorldVector3(skeletonFrame.Skeletons[i].Position);//data.Position);
                skeleton.ScreenPosition = GetDisplayPosition(skeletonFrame.Skeletons[i].Position);//data.Position);
                skeleton.Quality = skeletonFrame.Skeletons[i].Quality;//data.Quality;
                skeleton.TrackingID = skeletonFrame.Skeletons[i].TrackingID;//data.TrackingID;
                skeleton.TrackingState = skeletonFrame.Skeletons[i].TrackingState;//data.TrackingState;
                skeleton.UserIndex = skeletonFrame.Skeletons[i].UserIndex;//data.UserIndex;

                Dictionary<JointID, JointInfo> joints = new Dictionary<JointID, JointInfo>();

                // Process all joints
                if (SkeletonTrackingState.Tracked == skeletonFrame.Skeletons[i].TrackingState)//data.TrackingState)
                {
                    foreach (Joint joint in skeletonFrame.Skeletons[i].Joints)//data.Joints)
                    {                        
                        joints.Add(joint.ID, new JointInfo(GetDisplayPosition(joint.Position), GetWorldVector3(joint.Position), joint.TrackingState));                  
                    }
                }

                skeleton.Joints = joints;

                // Add this skeleton to the known ones
                m_skeletons[i] = skeleton;//Add(skeleton);
            } 
        }

        void nui_ColorFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            // 32-bit per pixel, RGBA image
            PlanarImage Image = e.ImageFrame.Image;

            byte[] newImage = new byte[Image.Width * Image.Height * Image.BytesPerPixel];

            // Change from BRGA- to RGBA-format
            for (int i = 0; i < newImage.Length; i += 4)
            {
                // Variable alpha level on reduced RGB mode
                if (ShowCameraImage == KinectCameraImage.ReducedRGB)
                {
                    newImage[i + 2] = (byte)(Image.Bits[i + 0] * m_hollowFrame[i / 4]);
                    newImage[i + 1] = (byte)(Image.Bits[i + 1] * m_hollowFrame[i / 4]);
                    newImage[i + 0] = (byte)(Image.Bits[i + 2] * m_hollowFrame[i / 4]);
                    newImage[i + 3] = (byte)(m_hollowFrame[i / 4] * 0xFF);
                }
                else // Full alpha level on normal RGB mode
                {
                    newImage[i + 2] = (byte)(Image.Bits[i + 0]);
                    newImage[i + 1] = (byte)(Image.Bits[i + 1]);
                    newImage[i + 0] = (byte)(Image.Bits[i + 2]);
                    newImage[i + 3] = 0xFF;
                }
            }

            m_texColor.SetData<byte>(newImage);
        }

        #endregion

        #region Private Methods

        Vector3 GetWorldVector3(Vector vector)
        {
            return new Vector3(vector.X, vector.Y, vector.Z);
        }
        
        // Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
        // that displays different players in different colors
        byte[] convertDepthFrame(byte[] depthFrame16)
        {
            int n = 0;

            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4)
            {
                int player = depthFrame16[i16] & 0x07;
                int depthPixelValue = (depthFrame16[i16 + 1] << 8) | depthFrame16[i16];
                int realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);

                // Transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));

                depthFrame32[i32 + RED_IDX] = 0;
                depthFrame32[i32 + GREEN_IDX] = 0;
                depthFrame32[i32 + BLUE_IDX] = 0;


                // If kinect mode is "ReducedRGB", create the hollow mask from the depth data
                // Attention: The depth image has 320x240 resolution, while the RGB image is VGA!!
                if (ShowCameraImage == KinectCameraImage.ReducedRGB)
                {
                    // Use the interal kinect camera calibration to calculate 
                    // the corrisponding RGB image pixel position from the depth pixel position.
                    int depthX = (i16 / 2) % 320;
                    int depthY = (i16 / 2) / 320;
                    int colorX, colorY;
                    nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, new ImageViewArea(), depthX, depthY, (short)depthPixelValue, out colorX, out colorY);
                    colorX = Math.Max(0, Math.Min(639, colorX));
                    colorY = Math.Max(0, Math.Min(479, colorY));

                    // Set if this pixel is part of a player
                    byte set = (player > 0) ? (byte)1 : (byte)0;

                    // Calculate the position in RGB image space
                    int position = colorX + colorY * 640;

                    // To adapt the resolution of the RGB image, map one depth pixel 
                    // into 4 pixels in the hollow mask.
                    m_hollowFrame[position] = set;
                    m_hollowFrame[position + 1] = set;

                    // The second line of the last line is unnessessary and
                    // causes an exception for range overflow
                    if ((position + 640) < m_hollowFrame.Length)
                    {
                        m_hollowFrame[position + 640] = set;
                        m_hollowFrame[position + 640 + 1] = set;
                    }
                }
                
                
                // Choose different display colors based on player
                switch (player)
                {
                    case 0:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 2);
                        break;
                    case 1:
                        depthFrame32[i32 + RED_IDX] = intensity;
                        break;
                    case 2:
                        depthFrame32[i32 + GREEN_IDX] = intensity;
                        break;
                    case 3:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 4:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 4);
                        break;
                    case 5:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 6:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 7:
                        depthFrame32[i32 + RED_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(255 - intensity);
                        break;
                }
            }

            return depthFrame32;
        }

        public Vector2 GetDisplayPosition(Vector vector)
        {            
            float depthX, depthY;
            nui.SkeletonEngine.SkeletonToDepthImage(vector, out depthX, out depthY);
            depthX = depthX * 320; //convert to 320, 240 space
            depthY = depthY * 240; //convert to 320, 240 space
            int colorX, colorY;
            ImageViewArea iv = new ImageViewArea();
            // only ImageResolution.Resolution640x480 is supported at this point
            nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);

            // map back to skeleton.Width & skeleton.Height
            return new Vector2((float)(colorX / 640f), (float)(colorY / 480f));
        }               

        #endregion

        #region Public Methods
             
        #endregion

        public void Dispose()
        {
            nui.Uninitialize();
            //Environment.Exit(0);
            //thread.Abort();            
        }
    }
}
#endif